#ifndef MAT_H_
#define MAT_H_

#include "../common.h"

#include "math_exception.h"
#include "Vec.h"
#include "aux_funcs.h"
#include <valarray>

namespace physicsworks {

namespace math {

using exception::EUndefMathOpt;
using exception::EBadArg;

template<class T, class Tr = real_traits<T> > class Mat {
protected:
	std::valarray<T> arr; /**< the matrix in low-level */
private:
	size_t sm; /** number of rows */
	size_t sn; /** number of columns */
	
	T diagonalMult() const {
		Assert<EUndefMathOpt>(!DMATH || sm == sn, "Only a square matrix has a main diagonal");
		
		T r = operator()(0,0);
		for (size_t i = 1; i < sm; ++i)
			r *= operator()(i,i);
		return r;
	}
	
	/**
	 * Eliminates all elements below the pivot in the same column.
	 */
	void elimiteBelowPivot(size_t pivot) {
		T a = -1 / (*this)(pivot, pivot);
		for (size_t r = pivot + 1; r < sm; ++r) {
			T mult = a * (*this)(r, pivot);

			for (size_t c = 0; c < pivot; ++c)
				(*this)(r,c) = mult * (*this)(pivot, c) + (*this)(r,c);
			(*this)(r,pivot) = 0;
			for (size_t c = pivot + 1; c < sm; ++c)
				(*this)(r,c) = mult * (*this)(pivot, c) + (*this)(r,c);
		}	
	}

public:
	typedef Tr traits_type;

	Mat<T, Tr>(size_t m, size_t n) : arr(m * n), sm(m), sn(n) {
		Assert<EBadArg>(!DMATH || m > 0 && m > 0, "The dimensions of a matrix must be greater than 0");
	}
	Mat<T, Tr>(const Mat<T, Tr>& m) : arr(m.arr) { sm = m.m(); sn = m.n(); }
	Mat<T, Tr>(size_t m, size_t n, const T v[]) : arr(m * n), sm(m), sn(n) {
		Assert<EBadArg>(!DMATH || m > 0 && m > 0, "The dimensions of a matrix must be greater than 0");
		for (size_t i = 0; i < sm*sn; ++i)
			arr[i] = v[i];
	}
	
	T operator () (size_t r, size_t c) const {
		Assert<EBadArg>(!DMATH || r >= 0 && c >= 0, "The indices must be non-negative");
		Assert<EBadArg>(!DMATH || r < sm && c < sn, "The row and column indices must be smaller than the row and column size");
		
		return arr[r * sn + c];
	}
	T& operator () (size_t r, size_t c) {
		Assert<EBadArg>(!DMATH || r >= 0 && c >= 0, "The indices must be non-negative");
		Assert<EBadArg>(!DMATH || r < sm && c < sn, "The row and column indices must be smaller than the row and column size");
		
		return arr[r * sn + c];
	}
	
	size_t m() const { return sm; }
	size_t n() const { return sn; }
	
	/**
	 * Returns the r-th row as Vec<T>.
	 */
	Vec<T> row(size_t r) const {
		Assert<EBadArg>(!DMATH || r >= 0, "The row index must be non-negative");
		Assert<EBadArg>(!DMATH || r < sm, "The row index must be smaller than the number of rows");
		
		Vec<T> v(sn);
		for (size_t i = 0; i < sn; ++i)
			v(i) = operator()(r, i);
		return v;
	}
	/**
	 * Sets the r-th row supplying a Vec<T>.
	 */
	void row(size_t r, const Vec<T>& v) {
		Assert<EBadArg>(!DMATH || r >= 0, "The row index must be non-negative");
		Assert<EBadArg>(!DMATH || r < sm, "The row index must be smaller than the number of rows");
		
		for (size_t i = 0; i < sn; ++i)
			operator()(r, i) = v(i);
	}
	
	/**
	 * Returns the c-th column as Vec<T>.
	 */
	Vec<T> column(size_t c) const {
		Assert<EBadArg>(!DMATH || c >= 0, "The column index must be non-negative");
		Assert<EBadArg>(!DMATH || c < sn, "The column index must be smaller than the number of columns");
		
		Vec<T> v(sm);
		for (size_t i = 0; i < sm; ++i)
			v(i) = operator()(i, c);
		return v;
	}
	/**
	 * Sets the c-th column supplying a Vec<T>.
	 */
	void column(size_t c, const Vec<T>& v) {
		Assert<EBadArg>(!DMATH || c >= 0, "The column index must be non-negative");
		Assert<EBadArg>(!DMATH || c < sn, "The column index must be smaller than the number of columns");
		
		for (size_t i = 0; i < sm; ++i)
			operator()(i, c) = v(i);
	}
	
	/**
	 * Returns the diagonal as Vec<T>.
	 */
	Vec<T> diagonal() const {
		Assert<EUndefMathOpt>(!DMATH || sm == sn, "Only a square matrix has a main diagonal");
		
		Vec<T> v(sm);
		for (size_t i = 0; i < sm; ++i)
			v(i) = operator()(i, i);
		return v;
	}
	/**
	 * Sets the diagonal as Vec<T>.
	 */
	Vec<T> diagonal(const Vec<T> v) {
		Assert<EUndefMathOpt>(!DMATH || sm == sn, "Only a square matrix has a main diagonal");
		Assert<EUndefMathOpt>(!DMATH || v.size() == sm, "The diagonal and the vector must have the same size");
		
		for (size_t i = 0; i < sm; ++i)
			operator()(i, i) = v(i);
	}
	
	/**
	 * Assigns another matrix.
	 */
	Mat<T, Tr>& operator = (const Mat<T, Tr>& m) {
		Assert<EUndefMathOpt>(!DMATH || sm == m.m() && sn == m.n(), "The matrices must have the same dimensions");
		
		arr = m.arr;
		return *this;
	}
	
	/**
	 * Assigns an array of reals representing another matrix.
	 */
	Mat<T, Tr>& operator = (const real* r) {
		for (size_t i = 0; i < arr.size(); ++i) arr[i] = r[i];
		return *this;
	}
	
	/**
	 * Sums by other matrix.
	 */
	const Mat<T, Tr> operator + (const Mat<T, Tr>& m) const {
		Mat<T, Tr> r = *this;
		return r += m;
	}
	/**
	 * Subtracts by other matrix.
	 */
	const Mat<T, Tr> operator - (const Mat<T, Tr>& m) const {
		Mat<T, Tr> r = *this;
		return r -= m;
	}
	
	/**
	 * Multiplies by a Vec<T>.
	 */
	const Vec<T> operator * (const Vec<T>& v) const {
		Assert<EUndefMathOpt>(!DMATH || sn == v.size(), "The vector' size must be equal to the number of columns of the matrix");
		
		Vec<T> vr(sn);
		for (size_t row = 0; row < sm; ++row) {
			T r = 0;
			for (size_t column = 0; column < sn; ++column) {
				r += operator()(row, column) * v(column);
			}
			vr(row) = r;
		}
		return vr;
	}
	
	/**
	 * Mutiplies by a scalar.
	 */
	const Mat<T, Tr> operator * (T v) const {
		Mat<T, Tr> r = *this;
		return r *= v;
	}
	
	/**
	 * Multiplies by other matrix and returns the result.
	 * Those matrices must be Amxn and Bnxo.
	 */
	const Mat<T, Tr> operator * (const Mat<T, Tr>& m) const {
		Assert<EUndefMathOpt>(!DMATH || sn == m.n(), "The number of columns of A matrix must be equal to the number of columns of B matrix");
		
		Mat<T, Tr> mr(sm, m.n()); // number of rows of A and columns of B
		for (size_t row = 0; row < sm; ++row) {
			for (size_t c = 0; c < m.n(); ++c) {
				T r = 0;
				for (size_t column = 0; column < sn; ++column) {
					r += operator()(row, column) * m(column, c);
				}
				mr(row, c) = r;
			}
		}
		return mr;
	}
	
	/**
	 * Sums by other matrix and assigns.
	 */
	Mat<T, Tr>& operator += (const Mat<T, Tr>& m) {
		Assert<EUndefMathOpt>(!DMATH || sm == m.m() && sn == m.n(), "The matrices must have the same dimensions");
		
		arr += m.arr;
		return *this;
	}
	/**
	 * Subtracts by other matrix and assigns.
	 */
	Mat<T, Tr>& operator -= (const Mat<T, Tr>& m) {
		Assert<EUndefMathOpt>(!DMATH || sm == m.m() && sn == m.n(), "The matrices must have the same dimensions");
		
		arr -= m.arr;
		return *this;
	}
	/**
	 * Multiplies by a scalar and assigns.
	 */
	Mat<T, Tr>& operator *= (T v) {
		arr *= v;
		return *this;
	}
	
	bool operator == (const Mat<T, Tr>& m) const {
		for (size_t i = 0; i < arr.size(); ++i)
			if ( !equal(arr[i], m.arr[i]) )
				return false;
		return true;
	}
	
	/**
	 * Swaps two rows.
	 */
	void swapRows(size_t r0, size_t r1) {
		Assert<EBadArg>(!DMATH || r0 >= 0 && r1 >= 0, "The row index must be non-negative");
		Assert<EBadArg>(!DMATH || r0 < sm && r1 < sm, "The row index must be smaller than the number of rows");
		Assert<EUndefMathOpt>(!DMATH || r0 != r1, "It only swaps two different rows");
		
		for (size_t c = 0; c < sn; ++c)
			std::swap( arr[r0 * sn + c], arr[r1 * sn + c] );
	}

	T determinant() const;
	bool solve(Vec<T>& sol);
	
	/**
	 * Returns an array of reals representing the matrix.
	 */
	real* toReal() const {
		real* r = new real[arr.size()];
		for (size_t i = 0; i < arr.size(); ++i) r[i] = arr[i];
		return r;
	}
	
	void zero() {
		for (size_t i = 0; i < arr.size(); ++i) arr[i] = 0;
	}
	
	// 
	/**
	 * Find the pivot with greater value and it's row.
	 * 
	 * \param pivotValue The greater pivot's value found.
	 * \param sPivot The first pivot to look up.
	 * \param ePivot The last pivot to look up.
	 * \return The pivot found.
	 */
	size_t findGreaterPivot(T& pivotValue, size_t sPivot, size_t ePivot) const {
		
		pivotValue = (*this)(sPivot, sPivot);
		size_t maxRow = sPivot;
		for (size_t r = sPivot + 1; r <= ePivot; ++r) {
			
			if (std::abs((*this)(r, sPivot)) > std::abs(pivotValue)) {
				pivotValue = (*this)(r, sPivot);
				maxRow = r;
			}
		}
		
		return maxRow;
	}
};

/**
 * Returns the determinant.
 * It uses forward elimination (through partial pivoting) to obtain an upper triangular
 * matrix then calculate its diagonal.
 */
template<class T, class Tr> T Mat<T, Tr>::determinant() const
{
	Assert<EUndefMathOpt>(!DMATH || sm == sn, "Only a square matrix has a determinant");
	
	size_t size = sm; // sm must be equal to sn
	T sign = 1;
	
	Mat<T, Tr> m(*this);
	
	// for each pivot, that is, for each Aii
	for (size_t pivot = 0; pivot < size; ++pivot) {
		
		T max;
		// find the pivot with greater value and it's row
		size_t maxRow = m.findGreaterPivot(max, pivot, size-1);
		
		// determinant is zero when a diagonal's element is zero
		if (equal<T>(max, 0)) return 0;
		
		if (maxRow != pivot) {
			m.swapRows(maxRow, pivot);
			sign *= -1; // remember: detA == -detB when B is A with 2 rows swapped
		}
		
		m.elimiteBelowPivot(pivot);
		
	}
	
	return sign * m.diagonalMult();
}

template<class T, class Tr> bool Mat<T, Tr>::solve(Vec<T>& sol)
{
	Assert<EBadArg>(!DMATH || sn-1 == sol.size(), "The solution vector must be the same size as the matrix's number of columns minus one");
	Assert<EBadArg>(!DMATH || sm+1 == sn, "The matrix's number of rows plus one must be equal to it's number of columns");
	
	size_t size = sm; // sm
	
	// for each pivot, that is, for each Aii
	for (size_t pivot = 0; pivot < size; ++pivot) {
		
		T max;
		// find the pivot with greater value and it's row
		size_t maxRow = findGreaterPivot(max, pivot, size-1);
		
		// multiples solutions for the variable of the position i are acceptable, return false
		if (equal<T>(max, 0)) return false;
		
		if (maxRow != pivot)
			swapRows(maxRow, pivot);
		
		elimiteBelowPivot(pivot);
		
	}
	
	// calculate the solution
	for (size_t p = sm - 1; p <= sm - 1; --p) {
		
		T r = 0;
		
		for (size_t vi = sm - 1; vi > p; --vi) {
			r += sol(vi) * (*this)(p, vi);
		}
		sol(p) = ( (*this)(p, sn-1) - r ) / (*this)(p, p);
	}
	
	return true;
}

/**
 * Returns the determinant.
 * It uses forward elimination to obtain an upper triangular matrix then calculate
 * its diagonal.
 * Without a pivoting strategy.
template<class T, class Tr = real_traits<T> > T Mat<T, Tr>::determinant() const
{
	Assert<EUndefMathOpt>(!DMATH || sm == sn, "Only a square matrix has a determinant");
	
	size_t size = sm; // sm must be equal to sn
	T sign = 1;
	
	Mat<T, Tr> m(*this);
	
	// for each pivot, that is, for each Aii
	for (size_t pivot = 0; pivot < size; ++pivot) {
		
		size_t r;
		
		if (m(pivot, pivot) == 0) {
			r = pivot;
			do {
				++r;
				if (r >= size) return 0; // it results in a zero element in the diagonal of
				                         // the upper triangular matrix
			} while ( equal(m(r, pivot), 0) );
			
			m.swapRows(r, pivot);
			sign *= -1; // remember: detA == -detB when B is A with 2 rows swapped
		}
		
		m.elimiteBelowPivot(pivot);
		
	}
	
	return sign * m.diagonalMult();
}
*/

}

/**
 * Prints the Mat<T, Tr> formatted.
 */
template<class T, class Tr> inline std::ostream& operator << (std::ostream& os, const math::Mat<T, Tr>& m)
{
	using std::ios_base;
	
	os.precision(4);
	os.setf(ios_base::fixed, ios_base::floatfield);
	
	os << "Mat: ";

	for (size_t row = 0; row < m.m(); ++row) {
		os << "\n     | ";
		for (size_t column = 0; column < m.n(); ++column) {
			os.width(10);
			os << m(row, column) << " ";
		}
		os << "|";
	}

	os.setf(ios_base::fmtflags(0), ios_base::floatfield); // resets to default
	
	return os;
}

}

#endif /*MAT_H_*/
